
//视觉模块初始化
void vision_init(){
  //分配内存
  wk = malloc(sizeof(WonderK210));
  //开启串口接收
  //这里不需要再调用初始化串口函数 Serial.begin(115200); 在 serial_begin() 函数里已经初始化了  
  wk->serial_begin();
  //分配内存
  result = malloc(sizeof(Find_Box_Msg_st));
}


//=======================================================================================
//=======================================================================================
//二维码检测部分代码~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=======================================================================================
//=======================================================================================
void qrcode_detection_loop(){
  while(qr_msg.qr_detection_loop){
    while( !qr_msg.recv_qr_msg_success ){
      //开始向视觉模块发送开始检测二维码命令
      if(qr_msg.recv_qr_msg_start == false){
        for(int i=0;i<1;i++){
          qr_msg.send_commond_msg = "qr_detect_start";
          Serial2.println(qr_msg.send_commond_msg);
          delay(200);
          for(int i=0;i<3;i++){
            qr_detect_start();
          }
        }      
      }
      //当接收到二维码后，检测视觉模块二维码反馈成功标志
      else{
        qr_msg.send_commond_msg = "qr_detect_success";
        Serial2.println(qr_msg.send_commond_msg);
        delay(200);
        for(int i=0;i<3;i++){
          qr_detect_success();
        }
      }
    }
    //Serial.println("break qr loop");
  }
  // Serial.print("x:");Serial.print(qr_msg.qr_x_pos);Serial.print(" | ");
  // Serial.print("y:");Serial.print(qr_msg.qr_y_pos);Serial.print(" | ");
  // Serial.print("w:");Serial.print(qr_msg.qr_w_pos);Serial.print(" | ");
  // Serial.print("h:");Serial.print(qr_msg.qr_h_pos);Serial.print(" | ");
  // Serial.print("msg:");Serial.println(qr_msg.qr_data);
}

//实时检测接收到视觉模块发送的信息
void qr_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_QRCODES);//获取消息
  if(rt)//若获取到新的消息
  {
    char str1[10] = "";
    String recv_data = "";
    strcpy(str1, result->msg);
    for(int i=0;i<sizeof(str1);i++){
      recv_data += str1[i];
    }
    int num = recv_data.compareTo("qr_success");
    if( num == 0  ){
      qr_msg.recv_qr_msg_success = true;
      qr_msg.qr_detection_loop = false;
    }
    wk->clear_rec();
  }
}

//检测上位机给下位机是否发送了二维码数据
void qr_detect_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_QRCODES);//获取消息
  if(rt)//若获取到新的消息
  {
    qr_msg.qr_x_pos = result->x;
    qr_msg.qr_y_pos = result->y;
    qr_msg.qr_w_pos = result->w;
    qr_msg.qr_w_pos = result->h;
    qr_msg.qr_data  = result->msg;
    qr_msg.recv_qr_msg_start = true;
    char str1[10] = "";
    strcpy(str1, result->msg);
    for(int i=0;i<sizeof(str1);i++){
      detect_msg.qr_complete_data += str1[i];
    }
    //处理二维码数据，将二维码对应的字符串数据转换为int类型数据
    process_qr_string_data(detect_msg.qr_complete_data);
    // detect_msg.qr_complete_data.trim();
    // int special_symbols = detect_msg.qr_complete_data.indexOf('+');
    // String data1 = detect_msg.qr_complete_data.substring(0, special_symbols);
    // String data2 = detect_msg.qr_complete_data.substring(special_symbols+1);
    // Serial.print("data1:");Serial.println(data1);
    // Serial.print("data2:");Serial.println(data2); 
    wk->clear_rec();
  }
  //wk->clear_rec();
}

//处理二维码数据，将二维码对应的字符串数据转换为int类型数据
void process_qr_string_data(String process_str){
  detect_msg.qr_complete_data.trim();
  int special_symbols = detect_msg.qr_complete_data.indexOf('+');
  String data1 = detect_msg.qr_complete_data.substring(0, special_symbols);
  String data2 = detect_msg.qr_complete_data.substring(special_symbols+1);
  // Serial.print("before_qr_str:");Serial.println(data1);
  // Serial.print("after_qr_str:");Serial.println(data2);
  detect_msg.before_the_plus_data = data1.toInt();
  detect_msg.after_the_plus_data = data2.toInt();
  // Serial.print("检测到的二维码信息为:");Serial.println(detect_msg.qr_complete_data);
  // Serial.print("二维码加号之前的数据:");Serial.println(detect_msg.before_the_plus_data);
  // Serial.print("二维码加号之后的数据:");Serial.println(detect_msg.after_the_plus_data);
}

void colorring_detection_loop(){
  colorring_msg.colorring_detection_loop = true;
  colorring_msg.recv_colorring_msg_success = false;
  colorring_msg.recv_colorring_msg_start = false;
  detect_msg.detection_colorring_sum_num ++;
  while(colorring_msg.colorring_detection_loop)
  {
    while(!colorring_msg.recv_colorring_msg_success)
    {
      if(colorring_msg.recv_colorring_msg_start == false)
      {
        colorring_msg.send_commond_msg = "color_ring_start";
        Serial2.println(colorring_msg.send_commond_msg);
        // Serial.println(colorring_msg.send_commond_msg);
        delay(200);
        colorring_detection_start();
      }
      else{
        colorring_msg.send_commond_msg = "color_ring_success";
        Serial2.println(colorring_msg.send_commond_msg);
        // Serial.println("send_color_ring_success");
        delay(200);
        for(int i=0;i<3;i++){
          colorring_detect_success();
        }
      }
    }
  }
  // Serial.println("colorring break");
  wk->clear_rec();
  delay(1000);
  switch(detect_msg.detection_colorring_sum_num){
    case 1: {detect_msg.recv_colorring_one   = detect_msg.receive_current_colorring;} break;
    case 2: {detect_msg.recv_colorring_two   = detect_msg.receive_current_colorring;} break;
    case 3: {detect_msg.recv_colorring_three = detect_msg.receive_current_colorring;} break;
    case 4: {detect_msg.recv_colorring_four  = detect_msg.receive_current_colorring;} break;
    case 5: {detect_msg.recv_colorring_five  = detect_msg.receive_current_colorring;} break;
    case 6: {detect_msg.recv_colorring_six   = detect_msg.receive_current_colorring;} break;
    default: break;
  }
}

void colorring_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_OBJECT);//获取消息
  if(rt)//若获取到新的消息
  {
    if( strcmp(result->msg, "colorring_success") == 0 ){
      colorring_msg.recv_colorring_msg_success = true;
      colorring_msg.colorring_detection_loop = false;
      wk->clear_rec();
    }
  }
}

void colorring_detection_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_FIND_OBJECT);//获取消息
  if(rt)//若获取到新的消息
  {
    colorring_msg.colorring_x_pos = result->x;
    colorring_msg.colorring_y_pos = result->y;
    colorring_msg.colorring_w_pos = result->w;
    colorring_msg.colorring_h_pos = result->h;
    colorring_msg.colorring_data  = result->msg;
    if(strcmp(result->msg, "red") == 0) {
      // Serial.print("I found red color ring. ");
      detect_msg.receive_current_colorring = detect_msg.RED_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    if(strcmp(result->msg, "green") == 0) {
      // Serial.print("I found green color ring. ");
      detect_msg.receive_current_colorring = detect_msg.GREEN_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    if(strcmp(result->msg, "blue") == 0) {
      // Serial.print("I found blue color ring. ");
      detect_msg.receive_current_colorring = detect_msg.BLUE_COLOR;
      colorring_msg.recv_colorring_msg_start = true;
    }
    wk->clear_rec();
  }
}
//=======================================================================================
//=======================================================================================
//色块检测部分代码~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//=======================================================================================
//=======================================================================================
//小车进入颜色检测循环，直到小车接收到颜色信息，才会退出该循环
void color_detection_loop(){
  color_msg.color_detection_loop = true;
  color_msg.recv_color_msg_success = false;
  color_msg.recv_color_msg_start = false;
  detect_msg.detection_color_sum_num ++;
  while(color_msg.color_detection_loop){
    while(!color_msg.recv_color_msg_success){
      if(color_msg.recv_color_msg_start == false){
        color_msg.send_commond_msg = "color_detect_start";
        Serial2.println(color_msg.send_commond_msg);
        // Serial.println(color_msg.send_commond_msg);
        delay(200);
        for(int i=0;i<3;i++){
          color_detect_start();
        }
      }
      else{
        color_msg.send_commond_msg = "color_detect_success";
        Serial2.println(color_msg.send_commond_msg);
        // Serial.println(color_msg.send_commond_msg);
        delay(200);
        for(int i=0;i<3;i++){
          color_detect_success();
        }
      }
    }
  }
  // Serial.print("x:");Serial.print(color_msg.color_x_pos);Serial.print(" | ");
  // Serial.print("y:");Serial.print(color_msg.color_y_pos);Serial.print(" | ");
  // Serial.print("w:");Serial.print(color_msg.color_w_pos);Serial.print(" | ");
  // Serial.print("h:");Serial.print(color_msg.color_h_pos);Serial.print(" | ");
  // Serial.print("msg:");Serial.println(color_msg.color_data);
  wk->clear_rec();
  delay(1000);
  switch(detect_msg.detection_color_sum_num){
    case 1: {detect_msg.recv_color_one = detect_msg.recv_current_color;} break;
    case 2: {detect_msg.recv_color_two = detect_msg.recv_current_color;} break;
    case 3: {detect_msg.recv_color_three = detect_msg.recv_current_color;} break;
    default: break;
  }
}

//检测上位机给下位机是否发送了颜色信息数据
void color_detect_start(){
  wk->update_data(); //更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);//获取消息
  if(rt)//若获取到新的消息
  {
    color_msg.color_x_pos = result->x;
    color_msg.color_y_pos = result->y;
    color_msg.color_w_pos = result->w;
    color_msg.color_w_pos = result->h;
    color_msg.color_data  = result->msg;
    if(strcmp(result->msg, "Redsth") == 0) {
      // Serial.print("I found red. ");
      detect_msg.recv_current_color = detect_msg.RED_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    if(strcmp(result->msg, "Greensth") == 0) {
      // Serial.print("I found green. ");
      detect_msg.recv_current_color = detect_msg.GREEN_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    if(strcmp(result->msg, "Bluesth") == 0) {
      // Serial.print("I found blue. ");
      detect_msg.recv_current_color = detect_msg.BLUE_COLOR;
      color_msg.recv_color_msg_start = true;
    }
    // color_msg.recv_color_msg_start = true;
    // Serial.print("x:");Serial.print(color_msg.color_x_pos);Serial.print(" | ");
    // Serial.print("y:");Serial.print(color_msg.color_y_pos);Serial.print(" | ");
    // Serial.print("w:");Serial.print(color_msg.color_w_pos);Serial.print(" | ");
    // Serial.print("h:");Serial.print(color_msg.color_h_pos);Serial.print(" | ");
    // Serial.print("msg:");Serial.println(color_msg.color_data);
    wk->clear_rec();
  }
}

//实时检测接收到视觉模块发送的信息
void color_detect_success(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);//获取消息
  if(rt)//若获取到新的消息
  {
    if( strcmp(result->msg, "color_success") == 0 ){
      color_msg.recv_color_msg_success = true;
      color_msg.color_detection_loop = false;
      wk->clear_rec();
    }
  }
}
